#include "SimpleTest.h"
#include <QtCharts/QLineSeries>
#include<qdebug.h>

SimpleTest::SimpleTest()
{
    connect(&timer, &QTimer::timeout, this, &SimpleTest::send_mavlink);
    timer.setInterval(500);
    timer.start();
    _mavlink = _toolbox->mavlinkProtocol();
    connect(_mavlink, &MAVLinkProtocol::messageReceived,        this, &SimpleTest::_mavlinkMessageReceived);
}

SimpleTest::~SimpleTest()
{
}

void SimpleTest::send_mavlink()
{
    qDebug()<<"time";
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    _vehiclelist=manager->vehicles();

    if(_vehiclelist && _vehiclelist->objectList()->count()>0)
    {

        for(int i=0;i<_vehiclelist->objectList()->count();i++)
        {
            QObject* obj =  _vehiclelist->get(i);
            Vehicle* vehicle0=static_cast<Vehicle*> (obj);
            //        if (vehicle0->id()==1)
            //        {
            WeakLinkInterfacePtr weakLink = vehicle0->vehicleLinkManager()->primaryLink();
            if (!weakLink.expired()) {
                SharedLinkInterfacePtr sharedLink = weakLink.lock();

                if (!sharedLink) {
                    qCDebug(VehicleLog) << "_handlePing: primary link gone!";
                    return;
                }
                //            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
                auto priority_link =sharedLink;
                mavlink_message_t msg;
                // qDebug()<<i;
                if(j%3==0)
                {
                    mavlink_msg_gps_raw_int_pack(vehicle0->id(),
                                                 vehicle0->id(),
                                                 &msg,
                                                 1,
                                                 1,
                                                 321399000-j*100,
                                                 1185291000,
                                                 1,
                                                 1,
                                                 1,
                                                 1,
                                                 1,
                                                 1,
                                                 1,
                                                 1,
                                                 1,
                                                 1,
                                                 1,
                                                 1);
                    vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
                }
                else if(j%3==1)
                {
                    mavlink_msg_set_gps_global_origin_pack(vehicle0->id(),
                                                           vehicle0->id(),
                                                           &msg,
                                                           1,
                                                           321399400,
                                                           1185291000,
                                                           1,
                                                           10222);
                    vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
                }
                else
                {
                    mavlink_msg_set_gps_global_origin_pack(vehicle0->id(),
                                                           vehicle0->id(),
                                                           &msg,
                                                           1,
                                                           321399800,
                                                           1185291000,
                                                           2,
                                                           10222);
                    vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);

                }
                mavlink_msg_adsb_vehicle_pack(vehicle0->id(),
                                              vehicle0->id(),
                                              &msg,
                                              1,
                                              -353632921,
                                              1491652874,
                                              1,
                                              1,
                                              1,
                                              1,
                                              1,
                                              "1",
                                              1,
                                              1,
                                              1,
                                              1);
                vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);

                j++;

            }
            //            }
        }
    }

}

void SimpleTest::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
    switch (message.msgid) {
    case MAVLINK_MSG_ID_GPS_RAW_INT:
        _handleGpsRawInt(message);
        break;
    }
}
void SimpleTest::_handleGpsRawInt(mavlink_message_t& message)
{
    mavlink_gps_raw_int_t gpsRawInt;
    mavlink_msg_gps_raw_int_decode(&message, &gpsRawInt);
    qDebug()<<"gpsRawInt.lat"<<gpsRawInt.lat;
}

void SimpleTest::setTest1(QString test)
{
_test1=test;
qDebug()<<test;
emit valueChanged();
}

void SimpleTest::setTest2(QString test)
{
_test2=test;
qDebug()<<test;
emit valueChanged2();
}
void SimpleTest::_test()
{
    qDebug()<<"in C++ slot";
    emit _testChanged();
}

